//% color="#ff9f06" iconWidth=50 iconHeight=40
namespace customNamespace {

    //% block="初始化" blockType="command"
    export function initialize(parameter: any, block: any) {
        Generator.addImport('import math');
    }

    //% block="将欧拉角 yaw:[YAW] pitch:[PITCH] roll:[ROLL] 转换为四元数并存储" blockType="command"
    //% YAW.shadow="number" YAW.defl=45
    //% PITCH.shadow="number" PITCH.defl=0
    //% ROLL.shadow="number" ROLL.defl=0
    export function eluer2qua(parameter: any, block: any) {
        var yaw = parameter.YAW.code;
        var pitch = parameter.PITCH.code;
        var roll = parameter.ROLL.code;
        Generator.addCode(`
yawRad = ${yaw} * math.pi / 180
pitchRad = ${pitch} * math.pi / 180
rollRad = ${roll} * math.pi / 180

sinp = math.sin(pitchRad / 2)
siny = math.sin(yawRad / 2)
sinr = math.sin(rollRad / 2)

cosp = math.cos(pitchRad / 2)
cosy = math.cos(yawRad / 2)
cosr = math.cos(rollRad / 2)

w = cosr * cosp * cosy + sinr * sinp * siny
x = sinr * cosp * cosy - cosr * sinp * siny
y = cosr * sinp * cosy + sinr * cosp * siny
z = cosr * cosp * siny - sinr * sinp * cosy

print('Quaternion:', [w, x, y, z])
        `);
    }

    //% block="将四元数 w:[W] x:[X] y:[Y] z:[Z] 转换为欧拉角并存储" blockType="command"
    //% W.shadow="number" W.defl=0
    //% X.shadow="number" X.defl=1
    //% Y.shadow="number" Y.defl=2
    //% Z.shadow="number" Z.defl=3
    export function qua2eluer(parameter: any, block: any) {
        var w = parameter.W.code;
        var x = parameter.X.code;
        var y = parameter.Y.code;
        var z = parameter.Z.code;
        Generator.addCode(`
row = math.atan2(2 * (${w} * ${x} + ${y} * ${z}), 1 - 2 * (${x} * ${x} + ${y} * ${y}))
pitch = math.asin(2 * (${w} * ${y} - ${z} * ${x}))
yaw = math.atan2(2 * (${w} * ${z} + ${x} * ${y}), 1 - 2 * (${y} * ${y} + ${z} * ${z}))

row = row * 180 / math.pi
pitch = pitch * 180 / math.pi
yaw = yaw * 180 / math.pi

print('Euler angles:', [yaw, pitch, row])
        `);
    }
}
